#include "./core.h"
#include "../xlib/compat.h"

namespace utils::core {

using ProcessFunc = void (*)();

template <typename T>
T read(uint32_t addr) {
    return *reinterpret_cast<volatile T *>(addr);
}

void run_process(uint32_t vector_addr) {
    __disable_irq();

    SysTick->CTRL = 0;
    SysTick->LOAD = 0;
    SysTick->VAL = 0;

    constexpr uint32_t COUNT = sizeof (NVIC->ICER) / sizeof (NVIC->ICER[0]);
    for (uint32_t i = 0; i < COUNT; i ++) {
        NVIC->ICER[i] = 0xFFFFFFFF;
        NVIC->ICPR[i] = 0xFFFFFFFF;
    }

    __enable_irq();

    ProcessFunc func = reinterpret_cast<ProcessFunc>( read<uint32_t>(vector_addr + 4) );
    __set_MSP( read<uint32_t>(vector_addr) );
    __set_CONTROL(0);
    func();
}

}
